import binascii
import logging
from ctypes import *
from zlgcan import *
from zuds_structure import *
import platform
import threading
import time
import datetime

# 配置logging
logging.basicConfig(level=logging.INFO, format='%(asctime)s - %(levelname)s - %(message)s')

# 全局变量，用于控制接收线程的启动和停止
START_RECEIVE_FLAG = 0

# 定义全局变量 zcanlib
zcanlib = ZCAN()

# 定义结构体用于存放UDS的参数内容
class PARAM_DATA(Structure):
    _pack_ = 1
    _fields_ = [("data", c_ubyte * 4096)]

# 定义通道参数结构体
class CHANNEL_PARAM(Structure):
    _fields_ = [
        ("channel_handle", c_uint32),  # 通道句柄
        ("Extend_Flag", c_uint32),  # 0-standard_frame, 1-extern_frame
        ("CANFD_type", c_uint32),  # 0-can,1-canfd
        ("trans_version", c_uint32)  # 0-ISO15765-2004,1-ISO15765-2016
    ]

# 发送回调函数，用于将UDS帧转换为CAN或CANFD帧并发送
@WINFUNCTYPE(c_uint32, POINTER(CHANNEL_PARAM), POINTER(ZUDS_FRAME), c_uint32)
def transmit_callback(ctx, frame, count):
    try:
        if ctx.contents.CANFD_type:
            zcanfd_data = (ZCAN_TransmitFD_Data * count)()
            memset(zcanfd_data, 0, sizeof(zcanfd_data))
            for i in range(count):
                zcanfd_data[i].frame.can_id = frame[i].id
                if ctx.contents.Extend_Flag:
                    zcanfd_data[i].frame.eff = 1
                zcanfd_data[i].frame.len = frame[i].data_len
                zcanfd_data[i].frame.__pad = 0x08  # 设置发送回显
                for j in range(frame[i].data_len):
                    zcanfd_data[i].frame.data[j] = frame[i].data[j]
            result = zcanlib.TransmitFD(ctx.contents.channel_handle, zcanfd_data, count)
        else:
            zcan_data = (ZCAN_Transmit_Data * count)()
            memset(zcan_data, 0, sizeof(zcan_data))
            for i in range(count):
                zcan_data[i].frame.can_id = frame[i].id
                if ctx.contents.Extend_Flag:
                    zcan_data[i].frame.eff = 1
                zcan_data[i].frame.can_dlc = frame[i].data_len
                for j in range(frame[i].data_len):
                    zcan_data[i].frame.data[j] = frame[i].data[j]
            result = zcanlib.Transmit(ctx.contents.channel_handle, zcan_data, count)
        return 0 if result == count else 1
    except Exception as e:
        logging.error(f"Error in transmit_callback: {e}")
        return 1

# ... 其他代码保持不变 ...

# UDS库类，封装了UDS库的操作
class ZUDS:
    def __init__(self):
        try:
            if platform.system() == "Windows":
                self._dll = windll.LoadLibrary("./zuds.dll")
            else:
                logging.warning("No support now!")
                self._dll = None
            if self._dll is None:
                raise FileNotFoundError("DLL couldn't be loaded!")
        except Exception as e:
            logging.error(f"Error loading DLL: {e}")

    def _call_dll_function(self, func_name, *args):
        func = getattr(self._dll, func_name, None)
        if func:
            try:
                return func(*args)
            except Exception as e:
                logging.error(f"Exception on {func_name}: {e}")
        return None

    def Uds_Init(self, type_):
        return self._call_dll_function("ZUDS_Init", type_)

    def Uds_SetTransmitHandler(self, zuds_handle, ctx):
        return self._call_dll_function("ZUDS_SetTransmitHandler", zuds_handle, ctx, transmit_callback)

    def Uds_Request(self, handle, request, response):
        return self._call_dll_function("ZUDS_Request", handle, byref(request), byref(response))

    def Uds_Onreceive(self, handle, uds_frame):
        return self._call_dll_function("ZUDS_OnReceive", handle, byref(uds_frame))

    def Uds_SetParam(self, handle, type_, param):
        return self._call_dll_function("ZUDS_SetParam", handle, type_, byref(param))

    def Uds_SetTesterPresent(self, handle, enable, param):
        return self._call_dll_function("ZUDS_SetTesterPresent", handle, enable, byref(param))

    def Uds_Release(self, handle):
        return self._call_dll_function("ZUDS_Release", handle)

    def Uds_Stop(self, handle):
        return self._call_dll_function("ZUDS_Stop", handle)


class UDSController:
    """
    UDSController类封装了UDS通信的主要操作，包括CAN设备的打开、UDS的初始化、请求的发送和响应的处理等。
    """
    def __init__(self,device_type=ZCAN_USBCANFD_200U, device_index=0,chn_index=0,abit=500000,bbit=2000000):
        """
        初始化UDSController类的实例。
        初始化ZCAN和ZUDS库的实例，并将设备句柄、通道句柄、UDS句柄等初始化为None。
        """
        self.zcanlib = ZCAN()
        self.zudslib = ZUDS()
        self.device_handle = None
        self.chn_handle_0 = None
        self.uds_handle = None
        self.chn_param = None
        self.read_thread_ = None
        self.device_type = device_type
        self.device_index = device_index
        self.chn_index = chn_index
        self.abit = abit
        self.bbit = bbit

    # 设置UDS参数的函数
    def Set_param(self, uds_handle, chn_param):
        """
        设置UDS的传输层和应用层参数。
        :param uds_handle: UDS句柄
        :param chn_param: 通道参数
        """
        try:
            # 传输层参数
            param_15765 = ZUDS_ISO15765_PARAM()
            memset(byref(param_15765), 0, sizeof(param_15765))
            param_15765.version = chn_param.trans_version
            param_15765.max_data_len = 64 if chn_param.trans_version else 8
            param_15765.local_st_min = 0
            param_15765.block_size = 8
            param_15765.fill_byte = 0
            param_15765.frame_type = chn_param.Extend_Flag
            param_15765.is_modify_ecu_st_min = 0
            param_15765.remote_st_min = 0
            param_15765.fc_timeout = 100
            param_15765.fill_mode = 1
            self.zudslib.Uds_SetParam(uds_handle, 1, param_15765)

            # 应用层参数
            param_seesion = ZUDS_SESSION_PARAM()
            memset(byref(param_seesion), 0, sizeof(param_seesion))
            param_seesion.timeout = 2000
            param_seesion.enhanced_timeout = 5000
            self.zudslib.Uds_SetParam(uds_handle, 0, param_seesion)
            logging.info("set_param completed")
        except Exception as e:
            logging.error(f"Error setting parameters: {e}")

    # 接收线程函数，用于接收CAN或CANFD数据并传递给UDS库处理
    def Read_Thread_Func(self, chn_handle, uds_handle):
        """
        接收线程函数，用于接收CAN或CANFD数据并传递给UDS库处理。
        :param chn_handle: 通道句柄
        :param uds_handle: UDS句柄
        """
        try:
            uds_frame = ZUDS_FRAME()
            while START_RECEIVE_FLAG:
                rcv_num = self.zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CAN)
                rcv_canfd_num = self.zcanlib.GetReceiveNum(chn_handle, ZCAN_TYPE_CANFD)
                if rcv_num:
                    rcv_msg, rcv_num = self.zcanlib.Receive(chn_handle, rcv_num, 1)
                    for i in range(rcv_num):
                        memset(byref(uds_frame), 0, sizeof(uds_frame))
                        uds_frame.id = rcv_msg[i].frame.can_id & 0x1fffffff
                        uds_frame.extend = rcv_msg[i].frame.eff
                        uds_frame.data_len = rcv_msg[i].frame.can_dlc
                        for j in range(uds_frame.data_len):
                            uds_frame.data[j] = rcv_msg[i].frame.data[j]
                        # logging.info(f"[{i}]: timestamps: {rcv_msg[i].timestamp}, type: CAN, id: {hex(rcv_msg[i].frame.can_id)}, dlc: {rcv_msg[i].frame.can_dlc}, eff: {rcv_msg[i].frame.eff}, rtr: {rcv_msg[i].frame.rtr}, data: {' '.join(hex(rcv_msg[i].frame.data[j]) for j in range(rcv_msg[i].frame.can_dlc))}")
                        self.zudslib.Uds_Onreceive(uds_handle, uds_frame)
                if rcv_canfd_num:
                    rcv_canfd_msgs, rcv_canfd_num = self.zcanlib.ReceiveFD(chn_handle, rcv_canfd_num, 1)
                    for i in range(rcv_canfd_num):
                        memset(byref(uds_frame), 0, sizeof(uds_frame))
                        uds_frame.id = rcv_canfd_msgs[i].frame.can_id & 0x1fffffff
                        uds_frame.extend = rcv_canfd_msgs[i].frame.eff
                        uds_frame.data_len = rcv_canfd_msgs[i].frame.len
                        for j in range(uds_frame.data_len):
                            uds_frame.data[j] = rcv_canfd_msgs[i].frame.data[j]
                        # logging.info(f"[{i}]: timestamp: {rcv_canfd_msgs[i].timestamp}, type: canfd,  id: {hex(rcv_canfd_msgs[i].frame.can_id)}, len: {rcv_canfd_msgs[i].frame.len}, eff: {rcv_canfd_msgs[i].frame.eff}, rtr: {rcv_canfd_msgs[i].frame.rtr}, esi: {rcv_canfd_msgs[i].frame.esi}, brs: {rcv_canfd_msgs[i].frame.brs}, data: {' '.join(hex(rcv_canfd_msgs[i].frame.data[j]) for j in range(rcv_canfd_msgs[i].frame.len))}")
                        self.zudslib.Uds_Onreceive(uds_handle, uds_frame)
                time.sleep(0.01)
            logging.info("exit receive")
        except Exception as e:
            logging.error(f"Error in Read_Thread_Func: {e}")

    # 打开CAN设备并初始化通道的函数
    def Open_CAN(self):
        """
        打开CAN设备并初始化通道。
        :return: 设备句柄和通道句柄
        """
        try:
            # 打开设备
            self.device_handle = self.zcanlib.OpenDevice(self.device_type, self.device_index, 0)
            if self.device_handle == INVALID_DEVICE_HANDLE:
                logging.error("Open CANFD Device failed!")
                return None, None
            logging.info(f"Open CANFD Device success! device handle: 0x{self.device_handle:x}.")

            # 初始化并启动通道，同时开启队列发送模式
            self.chn_handle_0 = canfd_start(self.zcanlib, self.device_handle, self.chn_index, self.abit, self.bbit)
            logging.info(f"chn handle: 0x{self.chn_handle_0:x}.")
            return self.device_handle, self.chn_handle_0
        except Exception as e:
            logging.error(f"Error opening CAN device: {e}")
            return None, None
        
    def handle_negative_response(response, recvData):
        """
        处理消极响应，记录日志并将相关数据添加到 recvData 列表中。
        :param response: UDS 响应结构体
        :param recvData: 用于存储接收数据的列表
        """
        logging.info(f"消极响应：{hex(response.response.negative.neg_code)} 服务号： {hex(response.response.negative.sid)}  消极码： {hex(response.response.negative.error_code)}")
        recvData.append(response.response.negative.neg_code)
        # 这里原代码可能有误，消极响应没有 positive 相关属性，推测应该是添加消极响应的 sid 和 error_code
        recvData.append(response.response.negative.sid)
        recvData.append(response.response.negative.error_code)

        return recvData

    def handle_positive_response(response, recvData):
        """
        处理积极响应，记录日志并将相关数据添加到 recvData 列表中。
        :param response: UDS 响应结构体
        :param recvData: 用于存储接收数据的列表
        """
        logging.info(f"积极响应,响应id:{hex(response.response.positive.sid)},参数长度:{response.response.positive.param_len},参数内容：{' '.join(hex(response.response.positive.param[i]) for i in range(response.response.positive.param_len))}")
        recvData.append(response.response.positive.sid)
        for i in range(response.response.positive.param_len):
             recvData.append(response.response.positive.param[i])

        return recvData
        

    # 发送UDS请求并处理响应的函数
    def requst_response(self, sid, data_1):
        """
        发送UDS请求并处理响应。
        :param sid: 服务ID
        :param data_1: 请求参数
        """
        try:
            request = ZUDS_REQUEST()
            memset(byref(request), 0, sizeof(request))
            request.src_addr = 0x774
            request.dst_addr = 0x674
            request.sid = sid
            request.suppress_response = 0
            request.param_len = len(data_1)
            param_data = PARAM_DATA()
            for i in range(request.param_len):
                param_data.data[i] = data_1[i]
            request.param = param_data.data
            response = ZUDS_RESPONSE()

            reqData = []
            reqData.append(sid)
            reqData.extend(data_1)


            self.zudslib.Uds_Request(self.uds_handle, request, response)
            memset(byref(param_data), 0, sizeof(param_data))

            recvData = []

            status_messages = {
                0: {
                    0: lambda recvData: self.handle_negative_response(response, recvData),
                    1: lambda recvData: self.handle_positive_response(response, recvData)
                },
                # 1: lambda: logging.info("响应超时"),
                # 2: lambda: logging.info("传输失败，请检查链路层，或请确认流控帧是否回复"),
                # 3: lambda: logging.info("取消请求"),
                # 4: lambda: logging.info("抑制响应"),
                # 5: lambda: logging.info("忙碌中"),
                # 6: lambda: logging.info("请求参数错误")
            }

            if response.status in status_messages:
                if response.status == 0:
                    if response.type in status_messages[0]:
                        status_messages[0][response.type]()
                else:
                    status_messages[response.status]()
            
            sendstr = binascii.b2a_hex(bytearray(reqData),' ')
            logging.info('Send UDS ID : [0x{0:X}]'.format(request.src_addr))
            logging.info('Send UDS request size :{0} ---,the send data are <<<{1}>>>'.format(len(reqData),sendstr))

            readstr = binascii.b2a_hex(bytearray(recvData),' ')
            logging.info('Read UDS reponse size :{0} ---,the read data are <<<{1}>>>\n'.format(len(recvData),readstr))
        except Exception as e:
            logging.error(f"Error in requst_response: {e}")

    # 初始化UDS的函数
    def init_uds(self):
        """
        初始化UDS。
        :return: UDS句柄、通道参数和接收线程
        """
        try:
            self.uds_handle = self.zudslib.Uds_Init(0)
            if self.uds_handle is None:
                raise ValueError("UDS初始化失败")
            logging.info(f"uds_handle: 0x{self.uds_handle:x}.")

            self.chn_param = CHANNEL_PARAM()
            self.chn_param.channel_handle = self.chn_handle_0
            self.chn_param.Extend_Flag = 0
            self.chn_param.CANFD_type = 1
            self.chn_param.trans_version = 1

            self.Set_param(self.uds_handle, self.chn_param)

            ret = self.zudslib.Uds_SetTransmitHandler(self.uds_handle, byref(self.chn_param))
            if ret != 0:
                raise ValueError("Uds_SetTransmitHandler 调用失败")
            logging.info("Uds_SetTransmitHandler success")

            global START_RECEIVE_FLAG
            START_RECEIVE_FLAG = 1
            self.read_thread_ = threading.Thread(target=self.Read_Thread_Func, args=(self.chn_handle_0, self.uds_handle))
            self.read_thread_.start()

            return self.uds_handle, self.chn_param, self.read_thread_
        except Exception as e:
            logging.error(f"初始化UDS时发生错误: {e}")
            return None, None, None

    # 关闭CAN设备和UDS库的函数
    def close_can(self):
        """
        关闭CAN设备和UDS库。
        """
        try:
            time.sleep(1)
            global START_RECEIVE_FLAG
            START_RECEIVE_FLAG = 0
            if self.read_thread_.is_alive():
                self.read_thread_.join()

            if self.uds_handle is not None:
                self.zudslib.Uds_Release(self.uds_handle)

            if self.chn_handle_0 is not None:
                ret = self.zcanlib.ResetCAN(self.chn_handle_0)
                if ret == 1:
                    logging.info("ResetCAN success! ")

            if self.device_handle is not None:
                ret = self.zcanlib.CloseDevice(self.device_handle)
                if ret == 1:
                    logging.info("CloseDevice success! ")
        except Exception as e:
            logging.error(f"关闭CAN设备时发生错误: {e}")

if __name__ == "__main__":
    uds_controller = UDSController()
    device_handle, chn_handle_0 = uds_controller.Open_CAN()
    uds_handle, chn_param, read_thread_ = uds_controller.init_uds()
    
    uds_controller.requst_response(0x10, [0x05])
    uds_controller.close_can()